#include "vehicle_lmpc_factory.hpp"
#include "test_vehicle_mpc_path.h"
using namespace auto_ros::control;
int main()
{
	std::string dyn_yaml_path_name = test_vehicle_mpc_path + "mpc_vy_yaw_error3dis/";
	std::string vehicle_lmpc_yaml_file = "vehicle_lmpc.yaml";
	VehicleLMpcFactory test_lmpc_factory;
	std::shared_ptr<VehicleLMpcBase> test_mpc =
		test_lmpc_factory.CreateSharedObject("VehicleVyYawError3disMpc",
											 dyn_yaml_path_name, vehicle_lmpc_yaml_file, 50);

	std::cout << test_mpc->vehicle_dyn_ptr_->dyn_yaml_ << std::endl;
	//bool VehicleLLMpc::step(const Eigen::VectorXd &x0,
	//					const Eigen::VectorXd &u_fb,
	//					const PlanningRefPoint &ref_point,
	//					double speed)
	Eigen::VectorXd x0(test_mpc->vehicle_dyn_ptr_->state_dim());
	x0 << 0.0, 0.0, 0.1, 0.0;
	Eigen::VectorXd u_fb(test_mpc->vehicle_dyn_ptr_->control_dim());
	u_fb << 0.0;
	PlanningRefPoint ref_point;
	ref_point.x_ = 0.0;
	ref_point.y_ = 0.0;
	ref_point.theta_ = 0.0;
	ref_point.kappa_ = 0.01;
	ref_point.v_ = 12.0;
	ref_point.s_ = 0.0;
	ref_point.a_ = 0.5;
	double speed = 10.0;
	// VehicleVyYawError3disMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
	//								double kappa, double speed)
	test_mpc->step(x0, u_fb, ref_point.kappa_, speed);
	/*test_mpc->step(x0,
				   u_fb,
				   ref_point,
				   speed);*/
	//std::cout << test_kf->state_esti() << std::endl;
}